#pragma once

#include "zuc/nml_intf/zuc.hh"
#include "libnml/nml/cmd_msg.hh"
#include "zuc/nml_intf/zucpos2.h"

/***************************** ROB STAT *******************************/
class ZUC_SENSOR_STAT
{
public:
    ZUC_SENSOR_STAT(){};

    SensorConfig sensorConfig;
    SensorStatus sensorStatus;
    // For internal NML/CMS use only.
    void update(CMS* cms);
};

class ZUC_TRAJ_STAT
{
public:
    ZUC_TRAJ_STAT();
    int status;  //FIXME:
    void update(CMS* cms);

    double linearUnits;   // units per mm
    double angularUnits;  // units per degree
    double cycleTime;     // cycle time, in seconds
    int joints;           // maximum joint number
    union
    {
        int deprecated_axes;
        int axes;  // maximum axis number
    };
    int axis_mask;                 // mask of axes actually present
    enum ZUC_TRAJ_MODE_ENUM mode;  // ZUC_TRAJ_MODE_FREE, ZUC_TRAJ_MODE_COORD
    bool enabled;                  // non-zero means enabled
    bool is_being_enabled;         // non-zero means is_being_enabled
    bool inpos;                    // non-zero means in position
    int queue;                     // number of pending motions, counting
    int activeQueue;               // number of motions blending
    bool queueFull;                // non-zero means can't accept another motion
    int id;                        // id of the currently executing motion
    bool paused;                   // non-zero means motion paused
    double scale;                  // velocity scale factor
    double rapid_scale;            // rapid scale factor

    ZucPose position_desired;  // 规划器规划的期望位置，最终位置可能被叠加修改
    ZucPose position;          // 最终指令位置，与发送给伺服的关节位置对应
    ZucPose actualPosition;    // 反馈位置，伺服反馈关节正解得到，在所选坐标系下
    ZucPose curSegTargetPose;
    double velocity;         // system velocity, for subsequent motions
    double acceleration;     // system acceleration, for subsequent motions
    double maxVelocity;      // max system velocity
    double maxAcceleration;  // system acceleration
    ServoMoveFilterParam servoMtnFltParam;

    int kinematics_type;  // identity=1,serial=2,parallel=3,custom=4
    int motion_type;
    double distance_to_go;  // in current move
    ZucPose dtg;
    double current_vel;  // in current move
    double curr_tcp_trans_vel;
    double curr_tcp_angular_vel;       // Tcp pose angular velocity
    double curr_tcp_rotation_axis[3];  // Tcp rotation axis
    double curr_elbow_vel;             //肘部速度

    double vel_tcp_angular;  //工具角速度
    double acc_tcp_angular;  //工具角加速度
    double acc_tcp_linear;   //工具加速度
    bool feed_override_enabled;
    bool admittance_enabled;
    double goalJointPos[ZUCMOT_MAX_JOINTS];
};

class ZUC_JOINT_STAT
{
public:
    ZUC_JOINT_STAT();
    int status;
    // For internal NML/CMS use only.
    void update(CMS* cms);

    int servo_version;
    int clsn_sensitivity;
    int first_poweron_date;

    // configuration parameters
    unsigned char jointType;  // ZUC_JOINT_LINEAR, ZUC_JOINT_ANGULAR
    double units;             // units per mm, deg for linear, angular
    double backlash;
    double minPositionLimit;
    double maxPositionLimit;
    double maxFerror;
    double minFerror;
    double maxVel;
    double maxAcc;
    double err_limit;

    double cmd_tor;
    double act_tor;
    // dynamic status
    double ferrorCurrent;   // current following error
    double ferrorHighMark;  // magnitude of max following error
    /*! \todo FIXME - is this really position, or the DAC output? */
    double desired_pos;          // motion 层的规划位置
    double output;               // commanded output position
    double input;                // current input position
    double velocity;             // current velocity
    double acceleration;         // current acceleration
    unsigned char inpos;         // non-zero means in position
    unsigned char homing;        // non-zero means homing
    unsigned char homed;         // non-zero means has been homed
    unsigned char fault;         // non-zero means axis amp fault
    unsigned char enabled;       // non-zero means enabled
    unsigned char minSoftLimit;  // non-zero means min soft limit exceeded
    unsigned char maxSoftLimit;  // non-zero means max soft limit exceeded
    unsigned char minHardLimit;  // non-zero means min hard limit exceeded
    unsigned char maxHardLimit;  // non-zero means max hard limit exceeded

    //伺服错误统计信息
    uint32_t servo_warn_date;  //统计日期
    unsigned int servo_warn_8384_cnt;
    unsigned int servo_warn_8384_daliy_cnt;
    unsigned int servo_warn_8385_cnt;
    unsigned int servo_warn_8385_daliy_cnt;
    unsigned int servo_warn_8387_cnt;
    unsigned int servo_warn_8387_daliy_cnt;
    unsigned int servo_warn_8388_cnt;
    unsigned int servo_warn_8388_daliy_cnt;
    unsigned int servo_warn_7001_cnt;
    unsigned int servo_warn_7001_daliy_cnt;
};

struct ZUC_MOTION_STAT
{
    void update(CMS* cms);
    RobJointData<ZUC_JOINT_STAT> joint;
    ZUC_TRAJ_STAT traj;
    ZUC_SENSOR_STAT sensor;           // sensor status parameter
    RobotModelData robot_model_data;  // capability of current robot model
    int status;                       // 运动状态，实际上是更新于traj.status

    uint32_t id;           // 多機器人時，標識機器人的id
    int robot_serial_num;  // 机器人序列号
    int robot_joint_num;   // 机器人关节数
    int powered_on;        // 机器人是否已上电
    int is_powering_on;    // 机器人是否正在上电
    bool is_enabled;       // 机器人是否已使能
    bool is_being_enable;  // 机器人是否正在使能

    RobotDH dhParam;

    uint8_t cur_payload_id;  // 机器人负载id
    double payload[4];       // 机器人负载参数
    uint8_t base_frame_id;
    int currentToolId;
    int currentUserId;

    int steppingWait;  // waiting for a step to complete
    //安全相关
    double dragTcpSpeedLimit;  // 拖拽速度限制值
    int on_soft_limit;
    int next_to_limit;
    int protective_stop;  // protective-stop mode
    int safetyguard_lock;
    int emergency_stop;  // emergency estop button
    int category_one_stop_program_pause;
    int estop_recovery;

    int percentage_mode_level;
    int is_reduce_mode;
    int full_dh_com;
    double elbow_center_offset;
    double elbow_sphere_radius;
    int dragNearLimit[ZUCMOT_MAX_JOINTS];
    int admit_axis_opt[CART_DIM];
    double admitCtrlConfig[CART_DIM];
    double percentage_mode_level1_val;
    double percentage_mode_level2_val;

    //辨识 力控相关
    short identifyWithLoad;       // 0:without-payload; 1:with-payload
    short identifyTrajIndex;      // index of traj to be identified
    short noloadIdentifyStat[3];  // -1:error, 0:not start, 1:ongoing, 2:complete
    short onloadIdentifyStat[3];  // -1:error, 0:not start, 1:ongoing, 2:complete
    double robotJointOffset[ZUCMOT_MAX_JOINTS];
    double torqueSensorSoftLimit[CART_DIM];
    double torqueSensorFilter;
    FtConfig admitFtConfig[CART_DIM];  // force or torque control configuration
    double drag_dead_zone[CART_DIM];   // 末端拖拽死区
    double velCompliantCtrl[5];
    double compliantCondition[CART_DIM];
    int compliantType;
    int compliantEnable;
    int ftFrame;
    int disableForceCtrl;
    int admitCtrlEnaFlag;
    Dynamics utilDynamics;  // utility variable for requested dynamics

    //其他
    CaptureStatus captureStatus[8];

    // return value for jaka app
    double utilJointPos[ZUCMOT_MAX_JOINTS];  // utility variable for requested cartesian position
    ZucPose utilCartPos;                     // utility variable for requested joint position
    Payload utilPayload;                     // utility variable for requested payload

    SafeAtt safeatt;
    double tcpError[3];
    uint8_t is_momentumLimited;
    uint8_t is_robot_power_limited;
    uint8_t is_safezone_limited;
    uint8_t is_tcpvel_limited;                       //关节运动中tcp限速是否生效
    uint8_t is_jointvel_limited[ZUCMOT_MAX_JOINTS];  //直线运动中关节限速是否生效标志
    int three_position_enable_limit;

    int errAddLine;  // save line of faile add to tp quene
    unsigned int safetyCRC;
    int maintenance_status;
    int backdriveStat;
    int forcelimitvalueStat;
    int collisiondetectionenableStat;
    short dynamicsIdentifyStat;  // -1:error, 0:not start, 1:ongoing, 2:moving complete, 3:calculate complete
    double identifyfrictionendposStat[ZUCMOT_MAX_JOINTS * ZUCMOT_MAX_JOINTS];
    double identifyfrictionstartposStat[ZUCMOT_MAX_JOINTS * ZUCMOT_MAX_JOINTS];
    short identifyfrictionStat;
    double dynamicsIdentifyTrajPara[250];
    double dynamicsIdentifyStartPos[ZUCMOT_MAX_JOINTS];
    short friction_identify_joint_index;
    double friction_identify_start_pos[ZUCMOT_MAX_JOINTS * ZUCMOT_MAX_JOINTS];
    double friction_identify_end_pos[ZUCMOT_MAX_JOINTS * ZUCMOT_MAX_JOINTS];
    double jointfrictioncompensateStat[ZUCMOT_MAX_JOINTS];
    double jointcollisionsensitivityStat[ZUCMOT_MAX_JOINTS];
    ZucPose carte_elbow_pos_cmd;

    double robot_real_power;
    double robot_real_momentum;
    int torque_control_enable;
    double gravity_rpy[3];

    double shrunkenPose[ZUCMOT_MAX_JOINTS];   //出厂姿态
    double stretchedPose[ZUCMOT_MAX_JOINTS];  //打开姿态
    double initialPose[ZUCMOT_MAX_JOINTS];    //安全姿态
    double initPoseTol;                       //安全姿态容差
    double homeVel;                           //回安全姿态速度倍率
};

void DHParam_update(CMS* cms, DHParam* x);
void Payload_update(CMS* cms, Payload* x);
void ModRtuComm_update(CMS* cms, ModRtuComm* x);
void ZucPose_update(CMS* cms, ZucPose* x);
void RobJointVal_update(CMS* cms, RobJointVal* x);
void FtConfig_update(CMS* cms, FtConfig* x);